#include "zf_common_headfile.h"
#include <cmath>
#include <stdint.h>
#include <string.h> // 需要添加memcpy的声明
#include <math.h>

#define Kp      10.0f                        // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki      0.008f                       // integral gain governs rate of convergence of gyroscope biases
#define halfT   0.005f                   // half the sample period,sapmple freq=500Hz
 
static float q0 = 1, q1 = 0, q2 = 0, q3 = 0;    // quaternion elements representing the estimated orientation
static float exInt = 0, eyInt = 0, ezInt = 0;    // scaled integral error
 
float yaw = 0;
float pitch = 0;
float roll = 0;

////////////////////////////////////////////////////////////////////////////////////////////////////////////

//知己利用陀螺仪原始数据计算出角度积分
float ZeroDrift_gyro_y=0;
float ZeroDrift_gyro_z=0;

int16 FJ_gyro_z = 0,FJ_gyro_y = 0;
volatile float FJ_Angle = 0;//最后引出的变量
volatile float FJ_Pitch = 0;//最后引出的变量
volatile float FJ_PitchSpeed = 0,FJ_LastPitchSpeed = 0;
volatile float FJ_LastAngleSpeed = 0,FJ_AngleSpeed = 0;
volatile float FJ_Angle_Max =  360;
volatile float FJ_Angle_Min = -360;
volatile float AngleSpeed = 0;

void Zero_Point_Detect(void)
{
    int i;
    float zero_point_y_accu=0;
    float zero_point_z_accu=0;
    for(i=0;i<=100;i++)//积累100次，求取平均值，获取当前零飘
    {
        imu660ra_get_gyro();
        zero_point_y_accu+=imu660ra_gyro_y;
        zero_point_z_accu+=imu660ra_gyro_z;
        system_delay_ms(3);
    }
    ZeroDrift_gyro_y=zero_point_y_accu/100.0;
    ZeroDrift_gyro_z=zero_point_z_accu/100.0;
}

void Gyroscope_GetData(void)
{
    int16 gyro_z =0;
    imu660ra_get_gyro();
    gyro_z=imu660ra_gyro_z;
    AngleSpeed = ((gyro_z-ZeroDrift_gyro_z) * GYRO_SENS)*DT;
}

//-------------------------------------------------------------------------------------------------------------------
//  @brief      陀螺仪计算航偏角角度
//  @param
//  @return     void
//-------------------------------------------------------------------------------------------------------------------
void Get_Gyroscope_Angle(void)
{
    float K=0.7;
    FJ_gyro_z = imu660ra_gyro_z;
    FJ_LastAngleSpeed=FJ_AngleSpeed;
    FJ_AngleSpeed += ((FJ_gyro_z-ZeroDrift_gyro_z) * GYRO_SENS)*DT;
    FJ_Angle = FJ_AngleSpeed*K+FJ_LastAngleSpeed*(1-K);                //向左为正
    FJ_Angle = FJ_Angle > FJ_Angle_Max ? FJ_Angle_Max : FJ_Angle;
    FJ_Angle = FJ_Angle < FJ_Angle_Min ? FJ_Angle_Min : FJ_Angle;
    FJ_Angle = -FJ_Angle;
}
//-------------------------------------------------------------------------------------------------------------------
//  @brief      陀螺仪计算俯仰角角度
//  @param
//  @return     void
//-------------------------------------------------------------------------------------------------------------------
int PoDao_Num=0;
float My_FJ_PitchSpeed=0;
float Last_FJ_PitchSpeed=0;
void Get_Gyroscope_Pitch(void)
{
    float K=0.7;
    FJ_LastPitchSpeed=FJ_PitchSpeed;
    FJ_gyro_y = imu660ra_gyro_y;
    FJ_PitchSpeed+=((FJ_gyro_y-ZeroDrift_gyro_y) * GYRO_SENS)*DT;
    FJ_Pitch = FJ_PitchSpeed*K+FJ_LastPitchSpeed*(1-K);

    FJ_Pitch = FJ_Pitch > 40 ? 40 : FJ_Pitch;
    FJ_Pitch = FJ_Pitch < (-40) ? (-40) : FJ_Pitch;
}
//-------------------------------------------------------------------------------------------------------------------
//  @brief      俯仰角角度清算
//  @param
//  @return     void
//  @note
//-------------------------------------------------------------------------------------------------------------------
void Clear_Gyroscope_Pitch(void)
{
    FJ_Pitch = 0;
    FJ_gyro_y = 0;
    FJ_PitchSpeed = 0;
    FJ_LastPitchSpeed = 0;
}
//-------------------------------------------------------------------------------------------------------------------
//  @brief      偏航角角度清算
//  @param
//  @return     void
//  @note
//-------------------------------------------------------------------------------------------------------------------
void Clear_Gyroscope_Angle(void)
{
    FJ_Angle = 0;
    FJ_gyro_z = 0;
    FJ_AngleSpeed = 0;
    FJ_LastAngleSpeed = 0;
}
 
void IMU_Update(float gx, float gy, float gz, float ax, float ay, float az)
{
    float norm;
    float vx, vy, vz;
    float ex, ey, ez;
    float temp0, temp1, temp2, temp3; 
 
    float q0q0 = q0 * q0;
    float q0q1 = q0 * q1;
    float q0q2 = q0 * q2;
    //float q0q3 = q0 * q3;
    float q1q1 = q1 * q1;
    //float q1q2 = q1 * q2;
    float q1q3 = q1 * q3;
    float q2q2 = q2 * q2;
    float q2q3 = q2 * q3;
    float q3q3 = q3 * q3;
 
    if (ax * ay * az == 0)
    {
        return;
    }
 
    norm = sqrt(ax * ax + ay * ay + az * az);       //
    ax = ax / norm;
    ay = ay / norm;
    az = az / norm;
 
    // estimated direction of gravity and flux (v and w)
    vx = 2 * (q1q3 - q0q2);
    vy = 2 * (q0q1 + q2q3);
    vz = q0q0 - q1q1 - q2q2 + q3q3 ;
 
    // error is sum of cross product between reference direction of fields and direction measured by sensors
    ex = (ay * vz - az * vy) ;
    ey = (az * vx - ax * vz) ;
    ez = (ax * vy - ay * vx) ;
 
    exInt = exInt + ex * Ki;
    eyInt = eyInt + ey * Ki;
    ezInt = ezInt + ez * Ki;
 
    // adjusted gyroscope measurements
    gx = gx + Kp * ex + exInt;
    gy = gy + Kp * ey + eyInt;
    gz = gz + Kp * ez + ezInt;
 
    // integrate quaternion rate and normalise
    temp0 = q0;
    temp1 = q1;
    temp2 = q2;
    temp3 = q3;
    q0 += (-temp1 * gx - temp2 * gy - temp3 * gz) * halfT;
    q1 += (temp0 * gx + temp2 * gz - temp3 * gy) * halfT;
    q2 += (temp0 * gy - temp1 * gz + temp3 * gx) * halfT;
    q3 += (temp0 * gz + temp1 * gy - temp2 * gx) * halfT;
 
    // normalise quaternion
    norm = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 = q0 / norm;
    q1 = q1 / norm;
    q2 = q2 / norm;
    q3 = q3 / norm;
 
    yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1) * 57.3; // unit:degree
    pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3; // unit:degree
    roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3; // unit:degree
}